(* Content-type: application/mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 7.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       145,          7]
NotebookDataLength[    134343,       3684]
NotebookOptionsPosition[    127831,       3481]
NotebookOutlinePosition[    128195,       3497]
CellTagsIndexPosition[    128152,       3494]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{

Cell[CellGroupData[{
Cell["2d kneed biped with torso", "Title",
 CellChangeTimes->{
  3.504390813710744*^9, {3.513011450427533*^9, 3.51301145527552*^9}, {
   3.513011745947039*^9, 3.513011749321201*^9}, {3.515858432663828*^9, 
   3.5158584360910234`*^9}, {3.5158700644438047`*^9, 
   3.5158700646998196`*^9}, {3.5158859650463057`*^9, 3.515885965342706*^9}}],

Cell[CellGroupData[{

Cell["initialization", "Section",
 CellChangeTimes->{
  3.504390830432091*^9, {3.513011606627004*^9, 3.513011611923624*^9}, {
   3.513011744096037*^9, 3.513011744363876*^9}, 3.514056528643134*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"t0", "=", 
   RowBox[{"AbsoluteTime", "[", "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.519646249630534*^9, 3.5196462513346314`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], ";"}], "\n", 
 RowBox[{"<<", "RobotLinks.m"}], "\n", 
 RowBox[{"<<", "Linearize.m"}], "\[IndentingNewLine]", 
 RowBox[{"SetDirectory", "[", 
  RowBox[{
   RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build\>\""}], 
  "]"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5194838161296067`*^9, 3.519483824366078*^9}, {3.543144396503061*^9, 
   3.5431443981981573`*^9}}],

Cell[BoxData[
 FormBox["\<\"C:\\\\Users\\\\rsinnet\\\\Documents\\\\MATLAB\\\\meen-652-biped\
\\\\code\\\\build\"\>", TraditionalForm]], "Output",
 CellChangeTimes->{3.543334119099128*^9, 3.543338405138275*^9, 
  3.5438645438581657`*^9, 3.543864640416689*^9, 3.544188145635123*^9, 
  3.544189542082139*^9, 3.544191504315967*^9, 3.5442065166993837`*^9, 
  3.544206568890369*^9, 3.5442070443945665`*^9, 3.5442075617171555`*^9, 
  3.5442629139200344`*^9, 3.5442630001809683`*^9, 3.5442632005174265`*^9, 
  3.5442735280880766`*^9, 3.5442948013908396`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["Discrete parameters", "Subsubsection",
 CellChangeTimes->{{3.5194813988383455`*^9, 3.519481400857461*^9}}],

Cell[CellGroupData[{

Cell["\<\
p[1] \[Rule] stance/non-stance state bit\
\>", "Item",
 CellChangeTimes->{{3.5196463257428875`*^9, 3.5196463490522203`*^9}}],

Cell["\<\
p[2] \[Rule] initial position of the hip\
\>", "Item",
 CellChangeTimes->{{3.519646349417241*^9, 3.5196463857983227`*^9}, {
  3.525812230366623*^9, 3.525812230710643*^9}}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"ParallelSimplify", "[", 
   RowBox[{"A_", "?", "MatrixQ"}], "]"}], ":=", 
  RowBox[{"ParallelTable", "[", 
   RowBox[{
    RowBox[{"Simplify", "[", 
     RowBox[{"A", "\[LeftDoubleBracket]", 
      RowBox[{"i", ",", "j"}], "\[RightDoubleBracket]"}], "]"}], ",", 
    RowBox[{"{", 
     RowBox[{"i", ",", 
      RowBox[{
       RowBox[{"Dimensions", "[", "A", "]"}], "\[LeftDoubleBracket]", "1", 
       "\[RightDoubleBracket]"}]}], "}"}], ",", 
    RowBox[{"{", 
     RowBox[{"j", ",", 
      RowBox[{
       RowBox[{"Dimensions", "[", "A", "]"}], "\[LeftDoubleBracket]", "2", 
       "\[RightDoubleBracket]"}]}], "}"}]}], "]"}]}], "\n", 
 RowBox[{
  RowBox[{"ParallelSimplify", "[", 
   RowBox[{"A_", "?", "VectorQ"}], "]"}], ":=", 
  RowBox[{"ParallelTable", "[", 
   RowBox[{
    RowBox[{"Simplify", "[", 
     RowBox[{"A", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
     "]"}], ",", 
    RowBox[{"{", 
     RowBox[{"i", ",", 
      RowBox[{"Length", "[", "A", "]"}]}], "}"}]}], "]"}]}], "\n", 
 RowBox[{
  RowBox[{"ParallelSimplify", "[", "A_", "]"}], ":=", 
  RowBox[{"Simplify", "[", "A", "]"}]}]}], "Input",
 CellChangeTimes->{{3.525444031603923*^9, 3.525444057323394*^9}, 
   3.525444167371688*^9, {3.5254442077009945`*^9, 3.5254442142373686`*^9}, {
   3.5254442656763105`*^9, 3.52544431862834*^9}, {3.5254446389836626`*^9, 
   3.525444643198904*^9}, {3.5273464706540365`*^9, 3.527346473775215*^9}, 
   3.527423798626493*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["model specification", "Section",
 CellChangeTimes->{{3.513531803717296*^9, 3.513531806022168*^9}, {
  3.51767999292592*^9, 3.5176799960260973`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"robotInfo", "=", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{"{", 
     RowBox[{
     "Lc", ",", "Lt", ",", "LT", ",", "rc", ",", "rt", ",", "rT", ",", "mh", 
      ",", "mt", ",", "mc", ",", "mf", ",", "g"}], "}"}], ",", 
    "\[IndentingNewLine]", 
    RowBox[{"{", 
     RowBox[{
     "347.98", ",", "261.112", ",", "0", ",", "282.37", ",", "128.15", ",", 
      "9.97", ",", 
      StyleBox["804.83",
       FontWeight->"Plain"], ",", "606.15", ",", "213.79", ",", "Null", ",", 
      "9810"}], "}"}]}], "}"}]}]], "Input",
 CellChangeTimes->{{3.5212045686835737`*^9, 3.521204606993765*^9}, {
   3.521204700067089*^9, 3.521204780986717*^9}, {3.5212145569110236`*^9, 
   3.5212145580280876`*^9}, {3.5212214210366507`*^9, 
   3.5212215159440784`*^9}, {3.521221549532*^9, 3.521221581995857*^9}, {
   3.5212225825050826`*^9, 3.5212225872403536`*^9}, {3.5212910875692406`*^9, 
   3.5212910900933847`*^9}, {3.5212911767343407`*^9, 
   3.5212911783804345`*^9}, {3.521291778077735*^9, 3.5212918161939154`*^9}, {
   3.521291985634607*^9, 3.521291991076918*^9}, {3.5212922081883364`*^9, 
   3.5212922159737816`*^9}, {3.5214849738620605`*^9, 3.521484978075301*^9}, {
   3.5214860083742313`*^9, 3.5214860164166913`*^9}, {3.522244545041182*^9, 
   3.522244546126244*^9}, {3.522244593734967*^9, 3.5222446610148153`*^9}, {
   3.522245182408637*^9, 3.5222451980385313`*^9}, {3.5222452480633926`*^9, 
   3.5222452884067*^9}, {3.5222453190144506`*^9, 3.522245320543538*^9}, {
   3.5222453895034823`*^9, 3.522245390398534*^9}, {3.5222454580864053`*^9, 
   3.5222454737583017`*^9}, {3.522508873288*^9, 3.522508878436*^9}, {
   3.52251692506606*^9, 3.5225169259961133`*^9}, {3.5226866514206*^9, 
   3.5226866515376*^9}, {3.5226870047536*^9, 3.5226870474105997`*^9}, {
   3.5226870857056*^9, 3.5226870879856*^9}, {3.5226871451626*^9, 
   3.5226871878106003`*^9}, 3.5230284498409986`*^9, {3.5230306460814857`*^9, 
   3.5230306462784967`*^9}, 3.523375153495903*^9, {3.5239834564467072`*^9, 
   3.5239834661022596`*^9}, {3.523984231265024*^9, 3.523984232481094*^9}, {
   3.5239849634039*^9, 3.52398496409194*^9}, 3.5239872789323406`*^9, {
   3.52398733818773*^9, 3.5239873424349732`*^9}, {3.5239874168672304`*^9, 
   3.5239874206984496`*^9}, {3.5239887549537644`*^9, 3.5239887551847773`*^9}, 
   3.52398922115843*^9, {3.5239892813588734`*^9, 3.523989283325986*^9}, {
   3.5239902778878717`*^9, 3.5239902785899115`*^9}, {3.5239955479933043`*^9, 
   3.523995556641799*^9}, {3.524246824429494*^9, 3.524246849028901*^9}, 
   3.524247077035942*^9, {3.5242480919719934`*^9, 3.524248100636489*^9}, {
   3.524248357066156*^9, 3.524248363905547*^9}, {3.5244002051058645`*^9, 
   3.5244002111532106`*^9}, {3.5244993533445044`*^9, 
   3.5244993688743925`*^9}, {3.524501441984967*^9, 3.5245014440170836`*^9}, {
   3.524502295655794*^9, 3.5245022998480344`*^9}, 3.5245104623769045`*^9, {
   3.5245105245994635`*^9, 3.524510543760559*^9}, {3.5245882608313823`*^9, 
   3.52458826166043*^9}, {3.5245883428200717`*^9, 3.524588343628118*^9}, {
   3.525090259555007*^9, 3.525090265970374*^9}, {3.525461165478421*^9, 
   3.5254611699576774`*^9}, {3.525812138101346*^9, 3.525812158210496*^9}, {
   3.525813170868417*^9, 3.5258131794839096`*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"Lc", "Lt", "LT", "rc", "rt", "rT", "mh", "mt", "mc", "mf", "g"},
     {"347.98`", "261.112`", "0", "282.37`", "128.15`", "9.97`", "804.83`", 
      "606.15`", "213.79`", "Null", "9810"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.5433341191101284`*^9, 3.5433384051672764`*^9, 
  3.543864543884167*^9, 3.54386464044069*^9, 3.5441881456801257`*^9, 
  3.544189542119141*^9, 3.5441915043783674`*^9, 3.544206568925371*^9, 
  3.5442070444225683`*^9, 3.5442075617451572`*^9, 3.544262913948036*^9, 
  3.5442630002739735`*^9, 3.5442632005614295`*^9, 3.5442735282820873`*^9, 
  3.5442948016478543`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"constsubs", "=", 
  RowBox[{
   RowBox[{"(", 
    RowBox[{
     RowBox[{
      RowBox[{"#1", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}], 
      "\[Rule]", 
      RowBox[{"Rationalize", "[", 
       FractionBox[
        RowBox[{"#1", "\[LeftDoubleBracket]", "2", "\[RightDoubleBracket]"}], 
        "1000"], "]"}]}], "&"}], ")"}], "/@", 
   RowBox[{"Transpose", "[", "robotInfo", "]"}]}]}]], "Input",
 CellChangeTimes->{{3.5197506180672736`*^9, 3.519750623492584*^9}, {
   3.5197506874442415`*^9, 3.519750758152286*^9}, {3.51975089075187*^9, 
   3.519750913356163*^9}, 3.521204776350452*^9, {3.5212215282457824`*^9, 
   3.521221530862932*^9}, {3.522245362849958*^9, 3.522245370044369*^9}, 
   3.5230284621307015`*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"{", 
   RowBox[{
    RowBox[{"Lc", "\[Rule]", 
     FractionBox["17399", "50000"]}], ",", 
    RowBox[{"Lt", "\[Rule]", 
     FractionBox["32639", "125000"]}], ",", 
    RowBox[{"LT", "\[Rule]", "0"}], ",", 
    RowBox[{"rc", "\[Rule]", 
     FractionBox["28237", "100000"]}], ",", 
    RowBox[{"rt", "\[Rule]", 
     FractionBox["2563", "20000"]}], ",", 
    RowBox[{"rT", "\[Rule]", 
     FractionBox["997", "100000"]}], ",", 
    RowBox[{"mh", "\[Rule]", 
     FractionBox["80483", "100000"]}], ",", 
    RowBox[{"mt", "\[Rule]", 
     FractionBox["12123", "20000"]}], ",", 
    RowBox[{"mc", "\[Rule]", 
     FractionBox["21379", "100000"]}], ",", 
    RowBox[{"mf", "\[Rule]", 
     FractionBox["Null", "1000"]}], ",", 
    RowBox[{"g", "\[Rule]", 
     FractionBox["981", "100"]}]}], "}"}], TraditionalForm]], "Output",
 CellChangeTimes->{3.543334119118129*^9, 3.5433384051962786`*^9, 
  3.5438645439061685`*^9, 3.5438646404586906`*^9, 3.5441881457471294`*^9, 
  3.5441895421591434`*^9, 3.544191504409567*^9, 3.544206568944372*^9, 
  3.5442070444425693`*^9, 3.5442075617671585`*^9, 3.5442629139720373`*^9, 
  3.5442630003109756`*^9, 3.544263200604432*^9, 3.5442735283140893`*^9, 
  3.544294801652855*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"ndof", " ", "=", " ", "5"}], ";"}]], "Input",
 CellChangeTimes->{{3.5158694971113553`*^9, 3.51586950751295*^9}, {
  3.515885996605161*^9, 3.5158859966675615`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"mm", "=", 
   RowBox[{
    RowBox[{"{", 
     RowBox[{"mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc"}], "}"}], "/.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"statesubs", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"x", "[", "i", "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["\[Theta]", "i"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"x", "[", 
         RowBox[{"i", "+", "ndof"}], "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], "]"}]}], 
  ";"}]}], "Input",
 CellChangeTimes->{{3.504464931380743*^9, 3.504464938859278*^9}, {
   3.50446501108097*^9, 3.504465011431731*^9}, {3.504465051759923*^9, 
   3.504465055474*^9}, {3.504617746752343*^9, 3.5046177488414*^9}, 
   3.504747296129977*^9, 3.50487586421991*^9, {3.504971551457563*^9, 
   3.50497170345489*^9}, {3.504971751954021*^9, 3.504971771812788*^9}, {
   3.5052271115942125`*^9, 3.5052271200806274`*^9}, {3.505231180364644*^9, 
   3.505231192454666*^9}, {3.50523314771557*^9, 3.5052331520835776`*^9}, {
   3.513011680554058*^9, 3.513011725186837*^9}, 3.51301182562513*^9, {
   3.513012344045216*^9, 3.513012345041097*^9}, {3.5158498204093294`*^9, 
   3.515849822884471*^9}, {3.515850217828061*^9, 3.5158502194581537`*^9}, {
   3.515850546875881*^9, 3.5158505634158273`*^9}, {3.515850617926945*^9, 
   3.515850624852341*^9}, {3.5158506825046387`*^9, 3.515850684811771*^9}, {
   3.5158510414861712`*^9, 3.5158510548999386`*^9}, {3.5158522964848585`*^9, 
   3.5158522987139854`*^9}, {3.5158524456553907`*^9, 
   3.5158524490995874`*^9}, {3.5158693992257566`*^9, 3.515869425043233*^9}, {
   3.515869486273735*^9, 3.515869512859256*^9}, {3.516707936034129*^9, 
   3.516707940618391*^9}, 3.516708850637441*^9}],

Cell["define inertia matrices", "Text",
 CellChangeTimes->{{3.516708878711047*^9, 3.516708881662216*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"For", "[", 
   RowBox[{
    RowBox[{"i", "=", "1"}], ",", 
    RowBox[{"i", "\[LessEqual]", 
     RowBox[{"Length", "[", "mm", "]"}]}], ",", 
    RowBox[{"i", "++"}], ",", 
    RowBox[{
     SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
     RowBox[{
      RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
      " ", 
      RowBox[{"DiagonalMatrix", "[", 
       RowBox[{"{", 
        RowBox[{"1", ",", "1", ",", "1"}], "}"}], "]"}]}]}]}], "]"}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.516708870317567*^9, 3.5167089298489723`*^9}, {
  3.5167089599876957`*^9, 3.516708960498725*^9}, {3.517697852846449*^9, 
  3.5176978537425003`*^9}}],

Cell["\<\
add inertia for torso (2.5 cm radius), the x,z terms will not show up in the \
EOM for this 2d model\
\>", "Text",
 CellChangeTimes->{{3.5167089723744044`*^9, 3.5167089780147266`*^9}, {
  3.516709130118427*^9, 3.5167091434381886`*^9}, {3.5167094517588234`*^9, 
  3.516709461902404*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "1"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1967374.33", "0", "12.37"},
         {"0", "1946798.09", 
          RowBox[{"-", "19.96"}]},
         {"12.37", 
          RowBox[{"-", "19.96"}], "119696.29"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "2"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"6494948.89", "58.80", "2024.23"},
         {"58.80", "6396011.40", "146697.91"},
         {"2024.23", "146697.91", "418371.07"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "3"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"3730232.81", 
          RowBox[{"-", "7.44"}], "1948.96"},
         {
          RowBox[{"-", "7.44"}], "518271.83", 
          RowBox[{"-", "1348.28"}]},
         {"1948.96", 
          RowBox[{"-", "1348.28"}], "3577190.08"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "4"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"6494948.89", 
          RowBox[{"-", "60.11"}], "1985.63"},
         {
          RowBox[{"-", "60.11"}], "6396011.38", 
          RowBox[{"-", "146697.91"}]},
         {"1985.63", 
          RowBox[{"-", "146697.91"}], "418371.06"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "5"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1967374.33", "0", 
          RowBox[{"-", "12.37"}]},
         {"0", "1946798.091", "19.96"},
         {
          RowBox[{"-", "12.37"}], "19.96", "119696.29"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.517697279088632*^9, 3.5176972829248514`*^9}, {
   3.5176973500646915`*^9, 3.5176974887036214`*^9}, {3.5176976110556197`*^9, 
   3.517697611375638*^9}, {3.520454104179551*^9, 3.5204542318028507`*^9}, {
   3.520454298738679*^9, 3.520454298807683*^9}, {3.520787636320943*^9, 
   3.520787636928978*^9}, {3.5207888553356667`*^9, 3.520788855551679*^9}, {
   3.5212216150807495`*^9, 3.5212218001203327`*^9}, {3.52122184116868*^9, 
   3.5212218551204786`*^9}, {3.5212923587829494`*^9, 
   3.5212923937079477`*^9}, {3.5214873144856634`*^9, 3.521487314869685*^9}, {
   3.5222446851841974`*^9, 3.5222446853692083`*^9}, {3.5222447190471344`*^9, 
   3.522244739155285*^9}, {3.522244896148264*^9, 3.5222449131612372`*^9}, {
   3.5225090343112*^9, 3.5225091254776*^9}, {3.5226873222595997`*^9, 
   3.5226873950976*^9}, 3.523027420015806*^9, {3.5233760202527485`*^9, 
   3.523376091120827*^9}, {3.5233762610257463`*^9, 3.523376327448327*^9}, {
   3.523376418297227*^9, 3.523376428528584*^9}, {3.5239880730617623`*^9, 
   3.5239880821322813`*^9}, {3.523988123796664*^9, 3.523988137267435*^9}, {
   3.523988338051919*^9, 3.5239883852996216`*^9}, {3.523988618537962*^9, 
   3.5239886371700277`*^9}, {3.5239889249444876`*^9, 
   3.5239889927513657`*^9}, {3.5242480213809557`*^9, 
   3.5242480706757755`*^9}, {3.5244990481420474`*^9, 3.524499082506013*^9}, {
   3.525812253419942*^9, 3.5258122823085938`*^9}, {3.525812449570161*^9, 
   3.525812454153423*^9}, 3.5258125549041853`*^9, {3.525812963823574*^9, 
   3.5258129641265917`*^9}}],

Cell[BoxData[
 RowBox[{"For", "[", 
  RowBox[{
   RowBox[{"i", "=", "1"}], ",", 
   RowBox[{"i", "\[LessEqual]", "5"}], ",", 
   RowBox[{"i", "++"}], ",", 
   RowBox[{
    SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Join", "[", 
       RowBox[{
        SubscriptBox["\[ScriptCapitalM]", "i"], ",", 
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", "2"}], "]"}], ",", 
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", 
        SubscriptBox["\[ScriptCapitalI]", "i"], ",", "2"}], "]"}], ",", "1"}],
      "]"}]}]}], "]"}]], "Input"],

Cell[BoxData[
 RowBox[{
  RowBox[{"p0", "=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["p", "x"], "'"}], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["p", "z"], "'"}], "[", "t", "]"}], "\[Rule]", "0"}]}], 
    "}"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.513532355258184*^9, 3.513532394514388*^9}, {
  3.5158587679930077`*^9, 3.515858770152131*^9}, {3.5167081862334394`*^9, 
  3.516708188824588*^9}}],

Cell[CellGroupData[{

Cell["generalized coordinates", "Subsection",
 CellChangeTimes->{
  3.504391119483818*^9, {3.513012126441401*^9, 3.51301212962473*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"q", "=", 
   RowBox[{"Table", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"dq", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "q"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"ddq", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "dq"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.504391169851249*^9, 3.504391191970865*^9}, {
   3.5129535326917257`*^9, 3.5129535355376797`*^9}, 3.513011937181409*^9, {
   3.513012138013254*^9, 3.513012139016991*^9}, 3.515858456069166*^9, {
   3.5158695182095623`*^9, 3.515869519316625*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"qe", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}], "}"}]}], "}"}], ",", "q"}], 
    "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"dqe", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "qe"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.513012131467762*^9, 3.513012222132253*^9}, 
   3.51353184821528*^9, 3.5158584591123405`*^9, {3.515859715694213*^9, 
   3.5158597176283236`*^9}}],

Cell[CellGroupData[{

Cell["location and direction of twists", "Subsubsection",
 CellChangeTimes->{
  3.504391222051317*^9, {3.513012004436311*^9, 3.513012017657574*^9}}],

Cell["positive z direction, ", "Text",
 CellChangeTimes->{{3.504895806007092*^9, 3.504895813878101*^9}, {
  3.504899673358274*^9, 3.504899680392001*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "0"], "=", 
   RowBox[{"{", 
    RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "px"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "pz"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q1"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q2"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q3"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q4"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q5"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.504391239055382*^9, 3.504391280546184*^9}, {
   3.504393690323417*^9, 3.504393694642943*^9}, {3.504394354402188*^9, 
   3.504394382731842*^9}, 3.504394560552597*^9, {3.504733962343795*^9, 
   3.50473399215855*^9}, {3.504734047207357*^9, 3.504734112723113*^9}, {
   3.504895790317638*^9, 3.504895793671604*^9}, {3.504895835178937*^9, 
   3.504895837468827*^9}, {3.50489969948502*^9, 3.504899711184298*^9}, {
   3.504901355868927*^9, 3.504901367126576*^9}, {3.504901644110264*^9, 
   3.504901651461383*^9}, {3.5125072468769054`*^9, 3.5125072492793093`*^9}, {
   3.512510523268261*^9, 3.512510626680843*^9}, {3.512510703620178*^9, 
   3.5125107228706117`*^9}, {3.5125107694990935`*^9, 
   3.5125107706690955`*^9}, {3.512510874416078*^9, 3.5125108760228806`*^9}, {
   3.5125109282169724`*^9, 3.5125109524594145`*^9}, 3.512511205857059*^9, {
   3.5125116217689886`*^9, 3.5125116245925937`*^9}, {3.512512838505127*^9, 
   3.5125128440899363`*^9}, 3.5125154382821627`*^9, 3.512515631401702*^9, {
   3.512515675315779*^9, 3.512515677234583*^9}, {3.512515713489046*^9, 
   3.51251571545465*^9}, {3.5125157856379766`*^9, 3.5125157895691843`*^9}, {
   3.5125160948139267`*^9, 3.512516101631139*^9}, {3.5125163059110966`*^9, 
   3.512516337906753*^9}, 3.5125169799917154`*^9, 3.512517350494175*^9, {
   3.5125175771075783`*^9, 3.512517615811246*^9}, {3.5125182182931213`*^9, 
   3.5125182275127373`*^9}, {3.513011948790205*^9, 3.513011986836761*^9}, {
   3.513012028383106*^9, 3.513012030110642*^9}, {3.513531858718806*^9, 
   3.513531893067239*^9}, {3.513869388669607*^9, 3.513869391129299*^9}, {
   3.513956274948754*^9, 3.513956281701611*^9}, {3.5155018106921597`*^9, 
   3.515501813883342*^9}, {3.515501850954462*^9, 3.5155018542846527`*^9}, {
   3.5158584657537203`*^9, 3.515858513238436*^9}, {3.515869263882015*^9, 
   3.5158692653601*^9}, {3.5158703231956043`*^9, 3.515870326599799*^9}, {
   3.515886006277178*^9, 3.5158860120335884`*^9}, {3.5167081937628703`*^9, 
   3.5167082227325273`*^9}, 3.51670884415207*^9, {3.544207033457941*^9, 
   3.5442070374011664`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["base configuration", "Subsubsection",
 CellChangeTimes->{
  3.504391314503995*^9, {3.513011998415469*^9, 3.513011998449444*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "1"]], "[", "0", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "-", "rc"}]}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "2"]], "[", "0", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt", "-", "rt"}]}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "3"]], "[", "0", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt", "+", "LT"}]}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "+", "Lt", "-", "rt"}]}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "5"]], "[", "0", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"Lc", "-", "rc"}]}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504391344841228*^9, {3.504393660794399*^9, 3.504393663580984*^9}, {
   3.504393700919354*^9, 3.504393704560237*^9}, 3.504477320678662*^9, 
   3.504477372811738*^9, {3.5125106491760826`*^9, 3.5125106821701403`*^9}, {
   3.5125108958281155`*^9, 3.512510897871719*^9}, {3.513012037683773*^9, 
   3.513012046816517*^9}, {3.51353190099301*^9, 3.513531902632749*^9}, {
   3.5158497160803623`*^9, 3.5158497388296633`*^9}, {3.5158505076696386`*^9, 
   3.5158505174832*^9}, {3.51585069440932*^9, 3.5158507070580435`*^9}, {
   3.5158510492876177`*^9, 3.5158510493766227`*^9}, {3.5158515449339666`*^9, 
   3.515851552726413*^9}, {3.5158516115196805`*^9, 3.5158516208742156`*^9}, {
   3.515852304074292*^9, 3.515852316172984*^9}, {3.5158524561829925`*^9, 
   3.515852474285028*^9}, {3.515859753607381*^9, 3.5158597547104445`*^9}, {
   3.515869286854329*^9, 3.5158692891464605`*^9}, {3.5158703778677316`*^9, 
   3.515870383570058*^9}, {3.5158860185856*^9, 3.5158860205200033`*^9}, {
   3.516707969937068*^9, 3.5167079805786767`*^9}, {3.5167082033634195`*^9, 
   3.516708212488941*^9}, 3.516708841559922*^9, {3.5167096385125055`*^9, 
   3.5167096393665543`*^9}, {3.517679210208151*^9, 3.5176792179195924`*^9}, {
   3.5176856517675877`*^9, 3.5176856652333574`*^9}, {3.5176858174780655`*^9, 
   3.5176858538481455`*^9}, {3.544207014455854*^9, 3.5442070293917084`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "1"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.516708682282812*^9, 3.5167087197409544`*^9}, 
   3.5167087910620337`*^9, 3.516708838777763*^9, 3.544207011669695*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "2"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.5167087252072673`*^9, 3.5167087354608536`*^9}, 
   3.516708790053976*^9, 3.5167088387897635`*^9, 3.5442070101656084`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "3"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.5167087432522993`*^9, 3.5167087881018643`*^9}, 
   3.5167088388017645`*^9, 3.5442070086065197`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "4"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.5167087493406477`*^9, 3.5167087863827662`*^9}, 
   3.5167088388117647`*^9, 3.5442070063343897`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]", "5"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q5"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.5167087714209104`*^9, 3.516708784944684*^9}, 
   3.5167088388187656`*^9, 3.544207004526286*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate the forward kinematics maps", "Subsubsection",
 CellChangeTimes->{
  3.504391365377686*^9, {3.513012058700656*^9, 3.513012059089004*^9}, {
   3.513013029501931*^9, 3.513013029568491*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "1"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "2"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "3"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "4"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "5"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q5"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.504391384027502*^9, 3.504391388680142*^9}, 
   3.504392263056063*^9, {3.504395252151757*^9, 3.504395255570185*^9}, {
   3.504733906298723*^9, 3.504733948089162*^9}, {3.504734137831782*^9, 
   3.504734159403184*^9}, {3.504734203476301*^9, 3.504734287191608*^9}, {
   3.504734420873801*^9, 3.504734554212083*^9}, {3.50473502117398*^9, 
   3.504735032877724*^9}, 3.504817876603461*^9, {3.5125107939599366`*^9, 
   3.5125108014011497`*^9}, 3.5125111628601837`*^9, {3.5125113835345707`*^9, 
   3.512511397605796*^9}, {3.513012063161425*^9, 3.513012087266791*^9}, {
   3.513012234208855*^9, 3.513012272695982*^9}, {3.513531912661093*^9, 
   3.513531929595589*^9}, {3.513532847587819*^9, 3.513532852574812*^9}, {
   3.515849743378924*^9, 3.515849789318551*^9}, {3.515850519247301*^9, 
   3.515850536609294*^9}, {3.5158507106542487`*^9, 3.5158507498544908`*^9}, {
   3.5158523190301476`*^9, 3.515852341076409*^9}, {3.5158524799643526`*^9, 
   3.515852528781145*^9}, {3.515858539342929*^9, 3.51585859216195*^9}, {
   3.515869305864416*^9, 3.515869315460965*^9}, {3.5158695563657446`*^9, 
   3.515869562919119*^9}, {3.5158703685702*^9, 3.5158703707433243`*^9}, 
   3.5158861184257755`*^9, {3.516707985323948*^9, 3.5167080036019936`*^9}, {
   3.516708230348963*^9, 3.516708236126293*^9}, {3.5442069969638534`*^9, 
   3.544207001988141*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stf"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stk"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "hip"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"Lc", "+", "Lt"}]}], "}"}]}], "]"}], "/.", "constsubs"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "torso"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"Lc", "+", "Lt", "+", "LT"}]}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsk"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";", 
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
    "constsubs"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504391344841228*^9, {3.504393660794399*^9, 3.504393663580984*^9}, {
   3.504393700919354*^9, 3.504393704560237*^9}, 3.504477320678662*^9, 
   3.504477372811738*^9, {3.5125106491760826`*^9, 3.5125106821701403`*^9}, {
   3.5125108958281155`*^9, 3.512510897871719*^9}, {3.513012037683773*^9, 
   3.513012046816517*^9}, {3.51353190099301*^9, 3.513531902632749*^9}, {
   3.513875671984485*^9, 3.513875737139051*^9}, {3.515858621125607*^9, 
   3.51585862956809*^9}, {3.5158693201992364`*^9, 3.515869321718323*^9}, {
   3.515886061969276*^9, 3.5158860734820957`*^9}, {3.516708239115464*^9, 
   3.5167082431056924`*^9}, {3.517679572905896*^9, 3.5176795767731175`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "stf"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "stk"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "hip"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "torso"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "nsk"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";", 
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q5"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504391344841228*^9, {3.504393660794399*^9, 3.504393663580984*^9}, {
   3.504393700919354*^9, 3.504393704560237*^9}, 3.504477320678662*^9, 
   3.504477372811738*^9, {3.5125106491760826`*^9, 3.5125106821701403`*^9}, {
   3.5125108958281155`*^9, 3.512510897871719*^9}, {3.513012037683773*^9, 
   3.513012046816517*^9}, {3.51353190099301*^9, 3.513531902632749*^9}, {
   3.513875795389027*^9, 3.513875935660706*^9}, 3.513876139346497*^9, {
   3.5158497919627028`*^9, 3.515849794085824*^9}, {3.515858606539773*^9, 
   3.5158586531084366`*^9}, {3.515869325403534*^9, 3.515869328852731*^9}, {
   3.5158695423689437`*^9, 3.515869545677133*^9}, {3.5158697121476545`*^9, 
   3.5158697210631647`*^9}, {3.5158702933078947`*^9, 
   3.5158703090517955`*^9}, {3.5158860863521185`*^9, 3.515886097818139*^9}, {
   3.515886139454612*^9, 3.5158861478630266`*^9}, {3.516708246161867*^9, 
   3.5167082520592046`*^9}, 3.5194838091162057`*^9, 3.525812570997106*^9, {
   3.54420698344808*^9, 3.544206992868619*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["aniplot", "Subsubsection",
 CellChangeTimes->{{3.51387566509846*^9, 3.513875667385709*^9}, {
  3.513876273981033*^9, 3.51387627490601*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"pos", "=", 
  RowBox[{
   RowBox[{
    RowBox[{"(", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "#1"], "[", "\[Theta]", "]"}], "&"}], ")"}], "/@", 
      
      RowBox[{"{", 
       RowBox[{
       "stf", ",", "stk", ",", "hip", ",", "torso", ",", "hip", ",", "nsk", 
        ",", "nsf"}], "}"}]}], ")"}], "\[LeftDoubleBracket]", 
    RowBox[{
     RowBox[{"1", ";;", "All"}], ",", 
     RowBox[{"{", 
      RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], "\[RightDoubleBracket]"}], 
   "\[Transpose]"}]}]], "Input",
 CellChangeTimes->{{3.513875953621905*^9, 3.513876010689337*^9}, {
  3.513876043248599*^9, 3.51387610532577*^9}, {3.513876169675573*^9, 
  3.513876183706996*^9}, {3.513887655923167*^9, 3.513887658548081*^9}, {
  3.513888227999695*^9, 3.513888239050703*^9}, {3.5158587000371203`*^9, 
  3.51585871925922*^9}, {3.5158693371802073`*^9, 3.5158693660608597`*^9}, {
  3.5158696092147675`*^9, 3.5158696155961323`*^9}, {3.515886157784644*^9, 
  3.515886172869871*^9}, {3.516708254867365*^9, 3.5167082553383923`*^9}, {
  3.543334138629245*^9, 3.5433341435505257`*^9}, {3.544262804184758*^9, 
  3.5442629372983713`*^9}, {3.544262974372492*^9, 3.5442629835390167`*^9}, {
  3.5442631950351133`*^9, 3.544263196187179*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"0", 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"17399", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], 
        "50000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
          ")"}]}], "250000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
          ")"}]}], "250000"]}], 
      RowBox[{"-", 
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"sin", "(", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
          ")"}]}], "250000"]}], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", "685"}], " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
          RowBox[{"514", " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"sin", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}], "-", 
             
             RowBox[{"sin", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
            ")"}]}]}], ")"}]}], "250000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", "685"}], " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "-", 
          RowBox[{"514", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "+", 
          
          RowBox[{"514", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
          
          RowBox[{"685", " ", 
           RowBox[{"sin", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"]},
     {"0", 
      FractionBox[
       RowBox[{"17399", " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "50000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{"cos", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}], "-", 
             
             RowBox[{"cos", "(", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], 
            ")"}]}]}], ")"}]}], "250000"], 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], "-", 
          
          RowBox[{"514", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
          
          RowBox[{"685", " ", 
           RowBox[{"cos", "(", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
         ")"}]}], "250000"]}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{{3.544262866020295*^9, 3.5442629377373967`*^9}, {
   3.544262979104763*^9, 3.544263004712228*^9}, {3.544263196532199*^9, 
   3.544263204746669*^9}, 3.5442735331773677`*^9, 3.544294805212058*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate center of mass", "Subsubsection",
 CellChangeTimes->{{3.515859381163079*^9, 3.5158593881694794`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["p", "COM"], "=", 
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{"(", 
       RowBox[{
        UnderoverscriptBox["\[Sum]", 
         RowBox[{"i", "=", "1"}], 
         RowBox[{"Length", "[", "mm", "]"}]], 
        RowBox[{
         RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}],
          " ", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
           "\[LeftDoubleBracket]", 
           RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
          RowBox[{"(", 
           RowBox[{"Total", "[", "mm", "]"}], ")"}]}]}]}], ")"}], "/.", 
      "constsubs"}], "//.", "p0"}], "//", "Simplify"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.515859389894578*^9, 3.515859428264773*^9}, {
   3.515859465154883*^9, 3.5158594906983433`*^9}, {3.515859551458819*^9, 
   3.515859552570883*^9}, {3.515859601883703*^9, 3.5158596025067387`*^9}, {
   3.515860689352903*^9, 3.5158607032496977`*^9}, 3.5158888908592005`*^9, {
   3.5158889225272565`*^9, 3.515888940763688*^9}, {3.515889859873678*^9, 
   3.5158898662696896`*^9}, {3.5159487112085867`*^9, 3.515948711738988*^9}, {
   3.5159488047463512`*^9, 3.5159488071019554`*^9}, {3.5159532190899763`*^9, 
   3.5159532199880276`*^9}, {3.515953578551536*^9, 3.51595358036364*^9}, {
   3.5159537466121483`*^9, 3.515953749921338*^9}, {3.5159854643716235`*^9, 
   3.515985465601694*^9}, {3.5167082669630575`*^9, 3.516708282715958*^9}, 
   3.5433341336949625`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate the manipulator inertia matrix", "Subsubsection",
 CellChangeTimes->{
  3.504392305735899*^9, 3.513012096077317*^9, {3.516708802974715*^9, 
   3.5167088033737383`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[ScriptCapitalD]e", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     UnderoverscriptBox["\[Sum]", 
      RowBox[{"i", "=", "1"}], 
      RowBox[{"Length", "[", "mm", "]"}]], 
     RowBox[{
      RowBox[{
       SubscriptBox["\[ScriptCapitalJ]", "i"], "\[Transpose]"}], ".", 
      SubscriptBox["\[ScriptCapitalM]", "i"], ".", 
      SubscriptBox["\[ScriptCapitalJ]", "i"]}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5167096143551235`*^9, 3.516709632103139*^9}, {
  3.543144406527634*^9, 3.5431444074086847`*^9}}],

Cell["\<\
project out the generalized coordinates defining the position of the stance \
foot to obtain the reduced \[ScriptCapitalD] matrix\
\>", "Text",
 CellChangeTimes->{{3.513012426370111*^9, 3.513012452229178*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[ScriptCapitalD]", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"\[ScriptCapitalD]e", "\[LeftDoubleBracket]", 
      RowBox[{
       RowBox[{"3", ";;", "All"}], ",", 
       RowBox[{"3", ";;", "All"}]}], "\[RightDoubleBracket]"}], "/.", 
     "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.513012422937284*^9, 3.513012423523879*^9}, {
   3.513012453927046*^9, 3.513012465021281*^9}, {3.513012554051636*^9, 
   3.513012587164631*^9}, {3.51353194017507*^9, 3.513531940256729*^9}, 
   3.515858726761649*^9, {3.515859543127342*^9, 3.5158595437243767`*^9}, 
   3.5158680079901824`*^9, {3.543144409407799*^9, 3.5431444101528416`*^9}, 
   3.5433341291077003`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate Coriolis matrix", "Subsubsection",
 CellChangeTimes->{{3.504392477990152*^9, 3.504392495055983*^9}, 
   3.513012592090603*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[ScriptCapitalC]", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"InertiaToCoriolis", "[", 
     RowBox[{"\[ScriptCapitalD]", ",", 
      RowBox[{"Flatten", "[", "q", "]"}], ",", 
      RowBox[{"Flatten", "[", "dq", "]"}]}], "]"}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{
  3.50439255557097*^9, {3.51295513827149*^9, 3.512955172679001*^9}, {
   3.5129552037364693`*^9, 3.512955231006535*^9}, {3.512955263739307*^9, 
   3.5129553328924294`*^9}, {3.512992901952466*^9, 3.512992920307856*^9}, {
   3.5129940359160023`*^9, 3.512994058469393*^9}, 3.5129940928271093`*^9, {
   3.512994138765036*^9, 3.5129941583899803`*^9}, {3.51301260617554*^9, 
   3.513012607311402*^9}, {3.513012641937742*^9, 3.513012652342953*^9}, {
   3.513012738154763*^9, 3.513012748062248*^9}, 3.513012780026892*^9, {
   3.51301313424288*^9, 3.513013136853251*^9}, {3.5431442470135107`*^9, 
   3.5431442478575587`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["\<\
calculate the potential energy and \[ScriptCapitalG] matrix\
\>", "Subsubsection",
 CellChangeTimes->{
  3.50439250607954*^9, {3.512994059885461*^9, 3.512994061579727*^9}, {
   3.513013138038511*^9, 3.513013138090329*^9}, 3.51301482840405*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"V", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"g", " ", 
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], 
        RowBox[{"Length", "[", "mm", "]"}]], 
       RowBox[{
        RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
        " ", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]}]}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"\[ScriptCapitalG]", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{"q", ",", "1"}], "}"}]], "V"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.504392530579445*^9, 3.504392568536552*^9}, {
  3.5052280833869843`*^9, 3.5052280856333885`*^9}, {3.5125114305562534`*^9, 
  3.512511430665453*^9}, {3.512994063656911*^9, 3.512994082683117*^9}, {
  3.513013142091812*^9, 3.513013228804931*^9}, {3.5158498432526364`*^9, 
  3.5158498449907355`*^9}, {3.543144312439253*^9, 3.5431443165244865`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["\<\
calculate the \[ScriptCapitalE] matrix for impact and the guard\
\>", "Subsubsection",
 CellChangeTimes->{
  3.50439250607954*^9, 3.504539201882091*^9, {3.513013067545057*^9, 
   3.513013077660357*^9}, {3.513015048236977*^9, 3.513015054334556*^9}, {
   3.513531976155762*^9, 3.513531976917461*^9}}],

Cell["the position of swing foot", "Text",
 CellChangeTimes->{{3.50453921178862*^9, 3.504539216085734*^9}, {
  3.50453924758814*^9, 3.504539276779558*^9}, {3.513531967714079*^9, 
  3.513531967756608*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
   RowBox[{"RPToHomogeneous", "[", 
    RowBox[{
     RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.513015087195842*^9, 3.513015090509893*^9}, {
   3.5167083039851747`*^9, 3.5167083075683794`*^9}, 3.516708384001751*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"ForwardKinematics", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q5"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.513015094248981*^9, 3.513015103484043*^9}, {
  3.513531984839549*^9, 3.513531986761064*^9}, {3.5158587372202473`*^9, 
  3.5158587445476665`*^9}, {3.5158696556534233`*^9, 3.5158696577865453`*^9}, {
  3.5158872712662287`*^9, 3.5158872784422407`*^9}, {3.5431443275721183`*^9, 
  3.5431443293872223`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[ScriptCapitalE]", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "qe", "]"}], ",", "1"}], "}"}]], 
     RowBox[{
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], 
      "\[RightDoubleBracket]"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.497873623815133*^9, 3.497873662882368*^9}, {
   3.497874034989651*^9, 3.4978740419880514`*^9}, 3.504539395679452*^9, 
   3.505228103167819*^9, {3.513013081342501*^9, 3.51301311670339*^9}, {
   3.513015040561296*^9, 3.513015044303407*^9}, 3.513534878131503*^9, {
   3.513873767730934*^9, 3.513873767796698*^9}, {3.5158587475438375`*^9, 
   3.5158587475908403`*^9}, {3.516708313664728*^9, 3.516708314824795*^9}, {
   3.54314433127433*^9, 3.543144332033373*^9}},
 FontSize->14],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"\[ScriptCapitalE]", "/.", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", ".2345"}], 
    ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", ".1894"}], 
    ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
     RowBox[{"-", ".293"}]}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", ".094"}], 
    ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
     RowBox[{"-", ".210"}]}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", ".923"}]}], 
   "}"}]}]], "Input",
 CellChangeTimes->{{3.5155049882169037`*^9, 3.5155049988325105`*^9}, {
  3.5155051086107893`*^9, 3.515505175018588*^9}, {3.515858751579068*^9, 
  3.5158587586594734`*^9}, {3.515862162917186*^9, 3.515862165572338*^9}, {
  3.5167096832590647`*^9, 3.516709690275466*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {"1", "0", 
      RowBox[{"-", "0.09544747668836438`"}], "0.2430085338559884`", 
      "0.4810099636462693`", 
      RowBox[{"-", "0.4810099636462693`"}], 
      RowBox[{"-", "0.22007570983164088`"}]},
     {"0", "1", 
      RowBox[{"-", "0.4481719887351099`"}], 
      RowBox[{"-", "0.3673165045277848`"}], 
      RowBox[{"-", "0.2599163371080736`"}], "0.2599163371080736`", 
      "0.26954918353076013`"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.5433384135207543`*^9, 3.543864553462715*^9, 
  3.5438646498312273`*^9, 3.544188157929826*^9, 3.5441895534707904`*^9, 
  3.5442065783899126`*^9, 3.544207053147067*^9, 3.544207570237643*^9, 
  3.5442630089904723`*^9, 3.5442632090689163`*^9, 3.5442735396647387`*^9, 
  3.5442948115834227`*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"h", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
     "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"hdot", " ", "=", "  ", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", "h", "]"}]}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.516708318191987*^9, 3.516708318664014*^9}, {
  3.516754669099207*^9, 3.516754685899168*^9}, {3.544206972585459*^9, 
  3.5442069789768248`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<jpos_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"pos", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513876298225372*^9, 3.513876314895954*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<D_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalD]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.512953878469522*^9, 3.512953886446665*^9}, {
   3.512994085488056*^9, 3.512994090994364*^9}, 3.512994135460699*^9, {
   3.513013423442089*^9, 3.513013424564995*^9}, {3.513014073959661*^9, 
   3.513014078429472*^9}, 3.513014297088332*^9, {3.513015201746145*^9, 
   3.513015202045883*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<C_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalC]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.512953878469522*^9, 3.512953886446665*^9}, {
   3.512994085488056*^9, 3.512994090994364*^9}, 3.512994135460699*^9, {
   3.513013423442089*^9, 3.513013424564995*^9}, {3.513014073959661*^9, 
   3.513014076194914*^9}, 3.513014294914336*^9, {3.513015198946243*^9, 
   3.513015199252923*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<G_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalG]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.512953878469522*^9, 3.512953886446665*^9}, {
   3.512994085488056*^9, 3.512994090994364*^9}, 3.512994135460699*^9, {
   3.513013423442089*^9, 3.513013424564995*^9}, {3.513014073959661*^9, 
   3.513014076194914*^9}, {3.513014291511217*^9, 3.51301431273912*^9}, {
   3.513015194622436*^9, 3.513015194950777*^9}, {3.5158609614944687`*^9, 
   3.515860974401207*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<De_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalD]e", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.512953878469522*^9, 3.512953886446665*^9}, {
   3.512994085488056*^9, 3.512994090994364*^9}, 3.512994135460699*^9, {
   3.513013423442089*^9, 3.513013424564995*^9}, {3.513014073959661*^9, 
   3.513014078429472*^9}, 3.513014297088332*^9, {3.51301484393123*^9, 
   3.513014845159042*^9}, {3.513015197361516*^9, 3.513015197908276*^9}, {
   3.513887638252694*^9, 3.513887640636993*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<E_mat\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalE]", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.4978952481411505`*^9, 3.4978953056554403`*^9}, {
   3.504541084026764*^9, 3.504541092118987*^9}, {3.504545862009512*^9, 
   3.504545864143497*^9}, 3.504652479388712*^9, {3.5125119377519436`*^9, 
   3.512511937845544*^9}, {3.512525693637813*^9, 3.512525694152614*^9}, {
   3.5125257246194677`*^9, 3.512525725415069*^9}, {3.513015036076006*^9, 
   3.513015036343996*^9}, {3.51301512396632*^9, 3.513015127387151*^9}, {
   3.513015189396898*^9, 3.513015190520248*^9}, {3.513887472707003*^9, 
   3.51388747514813*^9}, {3.513887645297176*^9, 3.513887645992246*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_sca\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"h", "/.", "statesubs"}]}], "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.4978952481411505`*^9, 3.4978953056554403`*^9}, {
   3.504541084026764*^9, 3.504541092118987*^9}, {3.504545862009512*^9, 
   3.504545864143497*^9}, 3.504652479388712*^9, {3.5125119377519436`*^9, 
   3.512511937845544*^9}, {3.512525693637813*^9, 3.512525694152614*^9}, {
   3.5125257246194677`*^9, 3.512525725415069*^9}, {3.513015036076006*^9, 
   3.513015036343996*^9}, {3.51301512396632*^9, 3.513015127387151*^9}, {
   3.513015181146682*^9, 3.513015186905603*^9}, {3.513887467972675*^9, 
   3.513887469003708*^9}, {3.513887648118017*^9, 3.513887648448756*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_dot_mat\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"hdot", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.4978952481411505`*^9, 3.4978953056554403`*^9}, {
   3.504541084026764*^9, 3.504541092118987*^9}, {3.504545862009512*^9, 
   3.504545864143497*^9}, 3.504652479388712*^9, {3.5125119377519436`*^9, 
   3.512511937845544*^9}, {3.512525693637813*^9, 3.512525694152614*^9}, {
   3.5125257246194677`*^9, 3.512525725415069*^9}, {3.513015036076006*^9, 
   3.513015036343996*^9}, {3.51301512396632*^9, 3.513015127387151*^9}, {
   3.513015181146682*^9, 3.513015186905603*^9}, {3.513887467972675*^9, 
   3.513887469003708*^9}, {3.513887648118017*^9, 3.513887648448756*^9}, {
   3.5167541423880806`*^9, 3.5167541451912413`*^9}}]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Linear Dynamics", "Section",
 CellChangeTimes->{{3.543008191642314*^9, 3.5430081951935167`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"\[Chi]", "=", 
   RowBox[{"Join", "[", 
    RowBox[{"q", ",", "dq"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"d\[Chi]", "=", 
   RowBox[{"D", "[", 
    RowBox[{"\[Chi]", ",", "t"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533323577435*^9, 3.513533331092033*^9}, {
  3.513533381181343*^9, 3.513533384041068*^9}, {3.513534180527118*^9, 
  3.513534186497912*^9}, {3.513534242697953*^9, 3.513534243136149*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"i\[ScriptCapitalD]", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"Inverse", "[", "\[ScriptCapitalD]", "]"}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.54300904896835*^9, 3.543009055347715*^9}, {
  3.543144474520523*^9, 3.5431444777977104`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[ScriptF]", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"Join", "[", 
     RowBox[{"dq", ",", 
      RowBox[{"i\[ScriptCapitalD]", ".", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"-", 
          RowBox[{"\[ScriptCapitalC]", ".", "dq"}]}], "-", 
         "\[ScriptCapitalG]"}], ")"}]}]}], "]"}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.543008451417172*^9, 3.5430084619807763`*^9}, {
   3.543008628596306*^9, 3.5430086354276967`*^9}, {3.543008859932538*^9, 
   3.543008866644922*^9}, {3.5430089298045344`*^9, 3.5430090650592704`*^9}, {
   3.543144336222613*^9, 3.543144344786103*^9}, 3.5431444371023827`*^9, 
   3.543144515386861*^9, 3.543144632992587*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[ScriptG]", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Table", "[", 
       RowBox[{"0", ",", 
        RowBox[{"{", "5", "}"}], ",", 
        RowBox[{"{", "5", "}"}]}], "]"}], ",", "i\[ScriptCapitalD]"}], "]"}], 
    "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.543008451417172*^9, 3.5430084619807763`*^9}, {
   3.543008628596306*^9, 3.5430086354276967`*^9}, {3.543008859932538*^9, 
   3.543008866644922*^9}, {3.5430089298045344`*^9, 3.5430090650592704`*^9}, {
   3.543144336222613*^9, 3.543144344786103*^9}, {3.5431444371023827`*^9, 
   3.543144450230134*^9}, 3.543144635642739*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Save", "[", 
  RowBox[{"\"\<vectorfields.eqn\>\"", ",", 
   RowBox[{"{", 
    RowBox[{"\[ScriptF]", ",", "\[ScriptG]"}], "}"}]}], 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build\>\""}], "]"}], 
  ";"}]}], "Input",
 CellChangeTimes->{{3.5431446460203323`*^9, 3.5431446864796467`*^9}, {
  3.543334077608754*^9, 3.543334096397829*^9}, {3.543338542696143*^9, 
  3.5433385429651585`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["feedback control", "Section",
 CellChangeTimes->{{3.513532015025627*^9, 3.513532018772655*^9}}],

Cell["we will make use of the chain rule as much as possible", "Text",
 CellChangeTimes->{{3.513534246341532*^9, 3.513534256526356*^9}, {
  3.513534308611278*^9, 3.513534359658739*^9}}],

Cell[CellGroupData[{

Cell[BoxData["q"], "Input",
 CellChangeTimes->{3.516754395266545*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "(", "t", ")"}]},
     {
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]},
     {
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]},
     {
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]},
     {
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.5433384700509872`*^9, 3.5438646047676497`*^9, 
  3.543864703541299*^9, 3.5441882372583637`*^9, 3.5441896303251867`*^9, 
  3.5442071049060273`*^9, 3.544207621618582*^9, 3.5442630604924183`*^9, 
  3.544263260450855*^9, 3.544273618277235*^9, 3.544294889194862*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  SubscriptBox["p", "hip"], "=", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], "\[LeftDoubleBracket]", 
    RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
   "p0"}]}], "\[IndentingNewLine]", 
 RowBox[{
  SubscriptBox["p", "hipdot"], "  ", "=", " ", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    SubscriptBox["\[PartialD]", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
    RowBox[{"Flatten", "[", 
     SubscriptBox["p", "hip"], "]"}]}], "]"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["v", "hip"], "=", 
   RowBox[{
    RowBox[{"D", "[", 
     RowBox[{
      SubscriptBox["p", "hip"], ",", "t"}], "]"}], "//", "Simplify"}]}], 
  ";"}]}], "Input",
 CellChangeTimes->{{3.513532470067184*^9, 3.513532510398695*^9}, {
   3.513532641208467*^9, 3.51353265065868*^9}, {3.513532881357095*^9, 
   3.513532897241124*^9}, {3.513887124348741*^9, 3.513887126504996*^9}, {
   3.5158548964365673`*^9, 3.515854897203611*^9}, 3.5158587723352556`*^9, 
   3.5158641632552757`*^9, {3.5167543264916115`*^9, 3.516754439052049*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"-", 
   FractionBox[
    RowBox[{"127", " ", 
     RowBox[{"(", 
      RowBox[{
       RowBox[{"685", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
       RowBox[{"514", " ", 
        RowBox[{"sin", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
      ")"}]}], "250000"]}], TraditionalForm]], "Output",
 CellChangeTimes->{3.543338470073989*^9, 3.543864604784651*^9, 
  3.5438647035643005`*^9, 3.544188237287365*^9, 3.5441896303751893`*^9, 
  3.544207104932029*^9, 3.544207621635583*^9, 3.544263060541421*^9, 
  3.544263260476856*^9, 3.5442736183062363`*^9, 3.544294889217863*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"{", 
   RowBox[{
    RowBox[{"-", 
     FractionBox[
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"685", " ", 
          RowBox[{"cos", "(", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
         RowBox[{"514", " ", 
          RowBox[{"cos", "(", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
        ")"}]}], "250000"]}], ",", 
    RowBox[{"-", 
     FractionBox[
      RowBox[{"32639", " ", 
       RowBox[{"cos", "(", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}], 
      "125000"]}], ",", "0", ",", "0", ",", "0"}], "}"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.543338470073989*^9, 3.543864604784651*^9, 
  3.5438647035643005`*^9, 3.544188237287365*^9, 3.5441896303751893`*^9, 
  3.544207104932029*^9, 3.544207621635583*^9, 3.544263060541421*^9, 
  3.544263260476856*^9, 3.5442736183062363`*^9, 3.5442948892388644`*^9}]
}, Open  ]],

Cell["time-invariant parameterization:", "Text",
 CellChangeTimes->{{3.513532550684887*^9, 3.513532556202501*^9}, {
  3.513532644709088*^9, 3.513532645133727*^9}, {3.5430096392661133`*^9, 
  3.5430096402181673`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"fb", "=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
     FractionBox[
      RowBox[{
       SubscriptBox["p", "hip"], "-", 
       RowBox[{"p", "[", "2", "]"}]}], 
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.513532403934621*^9, 3.513532467910504*^9}, {
  3.513532514383117*^9, 3.513532544860577*^9}, {3.51353289142399*^9, 
  3.513532893553784*^9}, {3.513533036340894*^9, 3.513533036810536*^9}, {
  3.513533928303517*^9, 3.513533928507984*^9}, {3.513534006139525*^9, 
  3.513534007303687*^9}, {3.51394646342299*^9, 3.513946465979686*^9}, {
  3.513946518081392*^9, 3.513946518637576*^9}, {3.515336393379112*^9, 
  3.515336393434178*^9}, {3.5158551745864763`*^9, 3.515855177569647*^9}, {
  3.519490960855955*^9, 3.519490960897957*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"\[Sigma]y", "=", 
  RowBox[{
   RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "fb"}]}]], "Input",
 CellChangeTimes->{{3.5158620968964095`*^9, 3.5158621015066733`*^9}, {
   3.51586213624666*^9, 3.515862144043106*^9}, 3.5158641571459265`*^9}],

Cell[BoxData[
 FormBox[
  FractionBox[
   RowBox[{
    RowBox[{"-", 
     RowBox[{"p", "(", "2", ")"}]}], "-", 
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"685", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
       ")"}]}], "250000"]}], 
   RowBox[{"a", "(", 
    RowBox[{"1", ",", "1"}], ")"}]], TraditionalForm]], "Output",
 CellChangeTimes->{3.543338470145993*^9, 3.5438646048576546`*^9, 
  3.5438647036383047`*^9, 3.5441882373593693`*^9, 3.5441896304651947`*^9, 
  3.5442067894359837`*^9, 3.544207105009033*^9, 3.5442076217105865`*^9, 
  3.5442630606144247`*^9, 3.5442632605488605`*^9, 3.5442736184012423`*^9, 
  3.5442948893038683`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[TextData[{
 "calculate ",
 Cell[BoxData[
  FormBox[
   SubscriptBox["y", "d"], TraditionalForm]]],
 " and its derivatives"
}], "Subsubsection",
 CellChangeTimes->{{3.513534419031216*^9, 3.513534436458221*^9}, {
  3.515862063545502*^9, 3.515862065419609*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"HumanFunction", "[", "i_", "]"}], ":=", 
   RowBox[{
    FractionBox[
     RowBox[{
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"i", ",", "1"}], "]"}], " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "2"}], "]"}], " ", 
         RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"i", ",", "3"}], "]"}], " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "2"}], "]"}], " ", 
         RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}]}], 
     RowBox[{"Exp", "[", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"i", ",", "4"}], "]"}], 
       RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]], "+", 
    RowBox[{"a", "[", 
     RowBox[{"i", ",", "5"}], "]"}]}]}], ";"}]], "Input",
 CellChangeTimes->{{3.515860352077612*^9, 3.5158603885236964`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"HF", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"HumanFunction", "[", "\[ScriptI]", "]"}], "/.", 
     "\[InvisibleSpace]", "fb"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"dHF", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{"Flatten", "[", "\[Chi]", "]"}], "}"}]], "HF"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"ddHF", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{"Flatten", "[", "\[Chi]", "]"}], "}"}]], 
     RowBox[{"(", 
      RowBox[{"dHF", ".", 
       RowBox[{"Flatten", "[", "d\[Chi]", "]"}]}], ")"}]}], "]"}]}], 
  ";"}]}], "Input",
 CellChangeTimes->{{3.544206613178902*^9, 3.54420669304747*^9}, {
  3.544206734576846*^9, 3.5442067603443193`*^9}, {3.544206797079421*^9, 
  3.5442067983644943`*^9}, {3.5442068405519075`*^9, 3.5442068552917504`*^9}, {
  3.5442068999833064`*^9, 3.5442069096998625`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["y", 
    RowBox[{"d", ",", "1"}]], "=", 
   RowBox[{"a", "[", 
    RowBox[{"1", ",", "1"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["y", 
    RowBox[{"d", ",", "2"}]], "=", 
   RowBox[{
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"HF", "/.", "\[InvisibleSpace]", 
        RowBox[{"\[ScriptI]", "\[Rule]", "i"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "2", ",", "ndof"}], "}"}]}], "]"}], "/.", 
    "\[InvisibleSpace]", "fb"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.515872287951982*^9, 3.5158723015107574`*^9}, {
  3.5158723371567965`*^9, 3.5158723885397353`*^9}, {3.515886208937134*^9, 
  3.51588621787595*^9}, {3.5158865763305807`*^9, 3.515886577094982*^9}, {
  3.516754046543599*^9, 3.5167540468006134`*^9}, {3.5433341055693536`*^9, 
  3.5433341059293747`*^9}, {3.5442067017069654`*^9, 3.5442067240412436`*^9}, {
  3.544206772345006*^9, 3.5442067838046618`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["Dy", 
    RowBox[{"d", ",", "1"}]], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{"Flatten", "[", "\[Chi]", "]"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      SubscriptBox["y", 
       RowBox[{"d", ",", "1"}]], "]"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["Dy", 
    RowBox[{"d", ",", "2"}]], "=", 
   RowBox[{"Table", "[", 
    RowBox[{
     RowBox[{"(", 
      RowBox[{"dHF", "/.", 
       RowBox[{"\[ScriptI]", "\[Rule]", "i"}]}], ")"}], ",", 
     RowBox[{"{", 
      RowBox[{"i", ",", "2", ",", "ndof"}], "}"}]}], "]"}]}], ";"}]}], "Input",\

 CellChangeTimes->{{3.513533175133389*^9, 3.513533227600235*^9}, {
   3.513534447833279*^9, 3.513534448428582*^9}, {3.513534581373526*^9, 
   3.51353459872282*^9}, {3.513534853366941*^9, 3.513534855172635*^9}, {
   3.513874463242996*^9, 3.513874465281417*^9}, {3.5158616236643424`*^9, 
   3.5158616650997124`*^9}, {3.5158617380638857`*^9, 3.5158617555558863`*^9}, 
   3.515862210846927*^9, {3.5158724149302444`*^9, 3.5158724662411795`*^9}, {
   3.5442067497847157`*^9, 3.5442067511517935`*^9}, {3.544206803904811*^9, 
   3.5442068307293453`*^9}, {3.5442068661363707`*^9, 3.544206883905387*^9}, {
   3.5442069330331964`*^9, 3.544206934817299*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["DLfy", 
    RowBox[{"d", ",", "1"}]], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      RowBox[{
       SubscriptBox["Dy", 
        RowBox[{"d", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["DLfy", 
    RowBox[{"d", ",", "2"}]], "=", 
   RowBox[{
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{"ddHF", "/.", "\[InvisibleSpace]", 
       RowBox[{"\[ScriptI]", "\[Rule]", "i"}]}], ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "2", ",", "ndof"}], "}"}]}], "]"}], "/.", 
    "\[InvisibleSpace]", "fb"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158617931210346`*^9, 3.5158618378455925`*^9}, 
   3.5158620205530434`*^9, 3.515862208155773*^9, {3.515872496206894*^9, 
   3.5158725099996824`*^9}, {3.5442068963590994`*^9, 3.5442069392495527`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate actual kinematics outputs and Jacobians", "Subsubsection",
 CellChangeTimes->{{3.513532922993929*^9, 3.513532938890856*^9}, {
  3.513534921064935*^9, 3.513534931010012*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  SubscriptBox["y", 
   RowBox[{"a", ",", "1"}]], "=", 
  RowBox[{
   SubscriptBox["\[PartialD]", "t"], 
   SubscriptBox["p", "hip"]}]}], "\n", 
 RowBox[{
  SubscriptBox["y", 
   RowBox[{"a", ",", "2"}]], "=", 
  RowBox[{"ParallelSimplify", "[", 
   RowBox[{
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       FractionBox[
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        UnderoverscriptBox["\[Sum]", 
         RowBox[{"i", "=", "1"}], "3"], 
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}]}], "}"}]}], "}"}], "/.",
     "\[InvisibleSpace]", "p0"}], "]"}]}]}], "Input",
 CellChangeTimes->{{3.513532586333204*^9, 3.513532677723161*^9}, {
   3.513532815648106*^9, 3.51353282124899*^9}, {3.513532861207259*^9, 
   3.513532869009141*^9}, {3.513532899901752*^9, 3.513532900419178*^9}, {
   3.51353311611145*^9, 3.513533116605757*^9}, 3.515521088606793*^9, {
   3.5155211643341246`*^9, 3.5155211667002597`*^9}, {3.5158526171702003`*^9, 
   3.515852621396442*^9}, {3.5158548842498703`*^9, 3.5158548875960617`*^9}, {
   3.5158551932005405`*^9, 3.5158551957126846`*^9}, {3.5158593451740203`*^9, 
   3.515859367792314*^9}, 3.51586024539751*^9, {3.5158611465420523`*^9, 
   3.515861146589055*^9}, 3.5158681409047847`*^9, {3.515869758310295*^9, 
   3.5158697588093233`*^9}, {3.515869821362901*^9, 3.515869822510967*^9}, {
   3.5158725172250957`*^9, 3.515872573737328*^9}, 3.5158863481049786`*^9, {
   3.515886571837773*^9, 3.5158865734445753`*^9}, {3.51595325150783*^9, 
   3.5159532575301747`*^9}, {3.5167083309077144`*^9, 
   3.5167083312677355`*^9}, {3.5167110381765614`*^9, 3.5167110528944035`*^9}, 
   3.5167111385673037`*^9, {3.5442069448218713`*^9, 3.5442069455499125`*^9}, {
   3.544207557718927*^9, 3.544207559446026*^9}}],

Cell[BoxData[
 FormBox[
  RowBox[{"-", 
   FractionBox[
    RowBox[{"127", " ", 
     RowBox[{"(", 
      RowBox[{
       RowBox[{"685", " ", 
        RowBox[{
         SuperscriptBox[
          SubscriptBox["\[Theta]", "1"], "\[Prime]",
          MultilineFunction->None], "(", "t", ")"}], " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], ")"}]}], "+", 
       RowBox[{"514", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "1"], "\[Prime]",
            MultilineFunction->None], "(", "t", ")"}], "+", 
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "2"], "\[Prime]",
            MultilineFunction->None], "(", "t", ")"}]}], ")"}], " ", 
        RowBox[{"cos", "(", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]}], ")"}]}]}], 
      ")"}]}], "250000"]}], TraditionalForm]], "Output",
 CellChangeTimes->{3.54333852078389*^9, 3.5438647613206043`*^9, 
  3.544188308693449*^9, 3.544189702558318*^9, 3.544207124064123*^9, 
  3.5442076407136736`*^9, 3.5442630798045225`*^9, 3.544263279635952*^9, 
  3.5442736424526176`*^9, 3.544294916979451*^9}],

Cell[BoxData[
 FormBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"-", "514"}], " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "-", 
        RowBox[{"685", " ", 
         RowBox[{"sin", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}], 
       RowBox[{
        RowBox[{"514", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}]}], ")"}]}], "+", 
        RowBox[{"685", " ", 
         RowBox[{"cos", "(", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "(", "t", ")"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]}], ")"}]}]}]]},
     {
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "(", "t", ")"}]},
     {
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "(", "t", ")"}]},
     {
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "(", "t", ")"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "(", "t", ")"}]}]}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], 
  TraditionalForm]], "Output",
 CellChangeTimes->{3.54333852078389*^9, 3.5438647613206043`*^9, 
  3.544188308693449*^9, 3.544189702558318*^9, 3.544207124064123*^9, 
  3.5442076407136736`*^9, 3.5442630798045225`*^9, 3.544263279635952*^9, 
  3.5442736424526176`*^9, 3.5442949170374546`*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"ya1", "=", 
   SubscriptBox["p", "hip"]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"ya2", "=", 
   SubscriptBox["y", 
    RowBox[{"a", ",", "2"}]]}], ";"}]}], "Input",
 CellChangeTimes->{{3.5431463779053907`*^9, 3.543146393518284*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{"SetDirectory", "[", 
  RowBox[{"NotebookDirectory", "[", "]"}], "]"}], "\[IndentingNewLine]", 
 RowBox[{"Save", "[", 
  RowBox[{"\"\<outputs.eqn\>\"", ",", 
   RowBox[{"{", 
    RowBox[{"ya1", ",", "ya2"}], "}"}]}], "]"}], "\[IndentingNewLine]", 
 RowBox[{"SetDirectory", "[", 
  RowBox[{
   RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build\>\""}], 
  "]"}]}], "Input",
 CellChangeTimes->{{3.5431458512872696`*^9, 3.5431458750146265`*^9}, {
  3.543146083104529*^9, 3.543146119713623*^9}, {3.5431461583298316`*^9, 
  3.543146159895921*^9}, {3.543146402860818*^9, 3.543146406463024*^9}, {
  3.543334054189415*^9, 3.543334070343339*^9}}],

Cell[BoxData[
 FormBox["\<\"C:\\\\Users\\\\rsinnet\\\\Documents\\\\MATLAB\\\\meen-652-biped\
\\\\code\"\>", TraditionalForm]], "Output",
 CellChangeTimes->{3.543338520876895*^9, 3.5438647614136095`*^9, 
  3.5441883088054557`*^9, 3.5441897026873255`*^9, 3.544207124148128*^9, 
  3.5442076407756777`*^9, 3.544263079884527*^9, 3.5442632796979556`*^9, 
  3.5442736425166216`*^9, 3.5442949170714564`*^9}],

Cell[BoxData[
 FormBox["\<\"C:\\\\Users\\\\rsinnet\\\\Documents\\\\MATLAB\\\\meen-652-biped\
\\\\code\\\\build\"\>", TraditionalForm]], "Output",
 CellChangeTimes->{3.543338520876895*^9, 3.5438647614136095`*^9, 
  3.5441883088054557`*^9, 3.5441897026873255`*^9, 3.544207124148128*^9, 
  3.5442076407756777`*^9, 3.544263079884527*^9, 3.5442632796979556`*^9, 
  3.5442736425166216`*^9, 3.544294917121459*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["Dy", 
    RowBox[{"a", ",", "1"}]], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      SubscriptBox["y", 
       RowBox[{"a", ",", "1"}]], "]"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["Dy", 
    RowBox[{"a", ",", "2"}]], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      SubscriptBox["y", 
       RowBox[{"a", ",", "2"}]], "]"}]}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533234344025*^9, 3.5135333201722*^9}, {
   3.513533374269628*^9, 3.513533375964947*^9}, 3.513534856986833*^9, 
   3.513874036445271*^9, {3.513947979870945*^9, 3.513948005409799*^9}, 
   3.515521160512906*^9, 3.5158669774602394`*^9, {3.515872580275702*^9, 
   3.515872610360423*^9}, {3.544206559095809*^9, 3.544206559394826*^9}, {
   3.544206959729724*^9, 3.54420696386596*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["DLfy", 
    RowBox[{"a", ",", "1"}]], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      RowBox[{
       SubscriptBox["Dy", 
        RowBox[{"a", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["DLfy", 
    RowBox[{"a", ",", "2"}]], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      RowBox[{
       SubscriptBox["Dy", 
        RowBox[{"a", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], 
  ";"}]}], "Input",
 CellChangeTimes->{{3.513533411525342*^9, 3.513533467774484*^9}, {
  3.513534859343821*^9, 3.513534864828146*^9}, {3.513874068222587*^9, 
  3.513874090913892*^9}, {3.513874132914815*^9, 3.513874149954311*^9}, {
  3.513874244874198*^9, 3.513874245301785*^9}, {3.513947988668254*^9, 
  3.513948001956114*^9}, {3.515521142307864*^9, 3.5155211535195055`*^9}, {
  3.515861859950857*^9, 3.515861881451087*^9}, {3.515861991803399*^9, 
  3.5158620021649914`*^9}, {3.5158656112430964`*^9, 3.515865614533284*^9}, {
  3.515872634222788*^9, 3.5158726502537045`*^9}, {3.5442069520162826`*^9, 
  3.5442069569535646`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["write the control files to disk", "Subsubsection",
 CellChangeTimes->{{3.513533556726437*^9, 3.513533560633779*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<sigma_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[Sigma]y", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
  3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
  3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
  3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
  3.513873093721922*^9}, {3.515862148936386*^9, 3.5158621546717143`*^9}, {
  3.516708335792994*^9, 3.516708336128013*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<phip_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["p", "hip"], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
  3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
  3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
  3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
  3.513873093721922*^9}, {3.5158698657024374`*^9, 3.5158698742209244`*^9}, {
  3.516708336825053*^9, 3.516708337152072*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<phip_dot_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["p", "hipdot"], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
  3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
  3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
  3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
  3.513873093721922*^9}, {3.5158698657024374`*^9, 3.5158698742209244`*^9}, {
  3.516708336825053*^9, 3.516708337152072*^9}, {3.5167541031348357`*^9, 
  3.516754114654495*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<pcom_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["p", "COM"], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158901103775215`*^9, 3.5158901154943304`*^9}, {
   3.515948100165904*^9, 3.515948106905116*^9}, {3.5159487206466036`*^9, 
   3.5159487213174047`*^9}, {3.51594878707152*^9, 3.5159487941383324`*^9}, {
   3.515953230430625*^9, 3.51595323192271*^9}, {3.515953761686011*^9, 
   3.515953766147266*^9}, 3.515985484570779*^9, {3.5167083379371166`*^9, 
   3.5167083382401342`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<yd1_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "1"}]], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158726672456765`*^9, 3.51587268496169*^9}, {
  3.5167083389361734`*^9, 3.516708341929345*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<yd2_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "2"}]], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, 
   3.513535978245256*^9, {3.5139480491379*^9, 3.513948080396936*^9}, {
   3.513948113545785*^9, 3.513948129942129*^9}, 3.5158619633557715`*^9, {
   3.5158726892259336`*^9, 3.515872692325111*^9}, {3.5167083517449064`*^9, 
   3.5167083521129274`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dyd1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "1"}]], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727027727084`*^9, 3.515872707855999*^9}, {
  3.5167083527929664`*^9, 3.5167083530889835`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dyd2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "2"}]], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533604687178*^9}, {
   3.513535690806333*^9, 3.513535693339207*^9}, 3.513535980161328*^9, 
   3.51586196000458*^9, {3.515872712110243*^9, 3.5158727149254036`*^9}, {
   3.516708353801024*^9, 3.5167083540880404`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfyd1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "1"}]], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.515861942956605*^9, 3.515861955946348*^9}, {
  3.515874032033738*^9, 3.515874045405503*^9}, {3.5167083552411065`*^9, 
  3.5167083555441236`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfyd2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "2"}]], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.515861942956605*^9, 3.515861955946348*^9}, {
  3.5158740329057875`*^9, 3.515874042230321*^9}, {3.5167083567361917`*^9, 
  3.5167083570722113`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<ya1_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "1"}]], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727241079288`*^9, 3.5158727304642925`*^9}, {
  3.516708359905373*^9, 3.5167083605204086`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<ya2_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "2"}]], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
  3.513535713498742*^9, 3.513535727403304*^9}, {3.5158727332724533`*^9, 
  3.5158727361286163`*^9}, {3.5167083610644393`*^9, 3.516708361376457*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dya1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "1"}]], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727417019353`*^9, 3.51587274563416*^9}, {
  3.5167083619924927`*^9, 3.5167083623125105`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dya2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "2"}]], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533604687178*^9}, {
  3.513535690806333*^9, 3.51353572494454*^9}, {3.515872742920005*^9, 
  3.5158727481833057`*^9}, {3.5167083632325635`*^9, 3.5167083635285807`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfya1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "1"}]], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533607692186*^9}, 
   3.513534673091028*^9, {3.513535695836862*^9, 3.513535722564655*^9}, {
   3.513536332459772*^9, 3.513536336316357*^9}, {3.5158727571118164`*^9, 
   3.515872763041156*^9}, {3.516708364688647*^9, 3.516708364976663*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfya2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "2"}]], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727601249886`*^9, 3.5158727645362415`*^9}, {
  3.516708366824769*^9, 3.516708367128786*^9}}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["PERL", "Section",
 CellChangeTimes->{{3.51353232609383*^9, 3.513532329542585*^9}}],

Cell["this next line assumes that perl is in your path", "Text",
 CellChangeTimes->{{3.513015230408543*^9, 3.513015241200741*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Run", "[", "\"\<perl math2mat.pl\>\"", "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build\>\""}], "]"}], 
  ";"}]}], "Input",
 CellChangeTimes->{{3.504476276692404*^9, 3.504476301348216*^9}, {
   3.504478868125564*^9, 3.504478869967337*^9}, {3.504538702237328*^9, 
   3.504538704315194*^9}, {3.504631487786531*^9, 3.504631500259924*^9}, {
   3.504632214564033*^9, 3.504632215194798*^9}, {3.504635416937057*^9, 
   3.50463541912094*^9}, {3.504874639115547*^9, 3.504874639679282*^9}, 
   3.504876166859266*^9, {3.505578303460942*^9, 3.5055783039289427`*^9}, {
   3.512922890945364*^9, 3.512922891900902*^9}, {3.5129244660544987`*^9, 
   3.51292449464812*^9}, {3.512925468730114*^9, 3.512925469241001*^9}, {
   3.513011661780643*^9, 3.513011661926162*^9}, {3.51301497676734*^9, 
   3.513015001300451*^9}, {3.5158700422275343`*^9, 3.515870043613613*^9}, {
   3.515870412159693*^9, 3.5158704163519325`*^9}, {3.5158863672774124`*^9, 
   3.5158863722382207`*^9}, {3.519483837591834*^9, 3.5194838377288427`*^9}, {
   3.519483891209901*^9, 3.5194838930990095`*^9}}],

Cell[BoxData[
 FormBox["0", TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5433385236860557`*^9, 3.5438647645817904`*^9, 3.544188312636675*^9, 
   3.5441897068175616`*^9, {3.5441914616973915`*^9, 3.544191502002163*^9}, 
   3.544207188216792*^9, 3.5442076435548363`*^9, 3.544263082825695*^9, 
   3.5442632824221115`*^9, 3.544273646603855*^9, 3.5442949207326655`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"t1", "=", 
   RowBox[{"AbsoluteTime", "[", "]"}]}], ";"}], "\n", 
 RowBox[{"Print", "[", 
  RowBox[{
   RowBox[{"ToString", "[", 
    RowBox[{"t1", "-", "t0"}], "]"}], "<>", "\"\< seconds elapsed.\>\""}], 
  "]"}]}], "Input"],

Cell[BoxData[
 FormBox["\<\"119.4588327 seconds elapsed.\"\>", TraditionalForm]], "Print",
 CellChangeTimes->{3.543338523712057*^9, 3.543864764606792*^9, 
  3.544188312669677*^9, 3.5441897068725643`*^9, 3.5442071882547946`*^9, 
  3.5442076435788374`*^9, 3.544263082872698*^9, 3.544263282456114*^9, 
  3.5442736466098557`*^9, 3.5442949207646675`*^9}]
}, Open  ]]
}, Open  ]]
}, Open  ]]
},
WindowSize->{1672, 923},
WindowMargins->{{Automatic, 0}, {Automatic, 0}},
ShowSelection->True,
FrontEndVersion->"8.0 for Microsoft Windows (64-bit) (November 7, 2010)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[CellGroupData[{
Cell[567, 22, 336, 5, 83, "Title"],
Cell[CellGroupData[{
Cell[928, 31, 196, 3, 71, "Section"],
Cell[1127, 36, 171, 4, 31, "Input"],
Cell[CellGroupData[{
Cell[1323, 44, 696, 15, 92, "Input"],
Cell[2022, 61, 550, 8, 30, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[2609, 74, 112, 1, 27, "Subsubsection"],
Cell[CellGroupData[{
Cell[2746, 79, 134, 3, 23, "Item"],
Cell[2883, 84, 181, 4, 23, "Item"]
}, Open  ]],
Cell[3079, 91, 1483, 37, 72, "Input"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[4611, 134, 154, 2, 71, "Section"],
Cell[CellGroupData[{
Cell[4790, 140, 3223, 52, 52, "Input"],
Cell[8016, 194, 1098, 23, 46, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[9151, 222, 752, 17, 47, "Input"],
Cell[9906, 241, 1245, 30, 48, "Output"]
}, Open  ]],
Cell[11166, 274, 198, 4, 31, "Input"],
Cell[11367, 280, 2183, 47, 52, "Input"],
Cell[13553, 329, 105, 1, 29, "Text"],
Cell[13661, 332, 693, 19, 31, "Input"],
Cell[14357, 353, 296, 6, 29, "Text"],
Cell[14656, 361, 3857, 94, 257, "Input"],
Cell[18516, 457, 831, 24, 31, "Input"],
Cell[19350, 483, 722, 22, 31, "Input"],
Cell[CellGroupData[{
Cell[20097, 509, 135, 2, 36, "Subsection"],
Cell[20235, 513, 783, 22, 72, "Input"],
Cell[21021, 537, 641, 20, 52, "Input"],
Cell[CellGroupData[{
Cell[21687, 561, 148, 2, 27, "Subsubsection"],
Cell[21838, 565, 153, 2, 29, "Text"],
Cell[21994, 569, 5131, 124, 172, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[27162, 698, 134, 2, 27, "Subsubsection"],
Cell[27299, 702, 3656, 89, 121, "Input"],
Cell[30958, 793, 1596, 47, 33, "Input"],
Cell[32557, 842, 1601, 47, 33, "Input"],
Cell[34161, 891, 1580, 47, 33, "Input"],
Cell[35744, 940, 1581, 47, 33, "Input"],
Cell[37328, 989, 1578, 47, 33, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[38943, 1041, 203, 3, 27, "Subsubsection"],
Cell[39149, 1046, 7375, 204, 121, "Input"],
Cell[46527, 1252, 2721, 77, 132, "Input"],
Cell[49251, 1331, 5675, 158, 132, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[54963, 1494, 145, 2, 27, "Subsubsection"],
Cell[CellGroupData[{
Cell[55133, 1500, 1296, 29, 31, "Input"],
Cell[56432, 1531, 9896, 274, 70, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[66377, 1811, 117, 1, 27, "Subsubsection"],
Cell[66497, 1814, 1548, 34, 56, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[68082, 1853, 184, 3, 27, "Subsubsection"],
Cell[68269, 1858, 569, 14, 56, "Input"],
Cell[68841, 1874, 219, 4, 29, "Text"],
Cell[69063, 1880, 746, 15, 31, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[69846, 1900, 142, 2, 27, "Subsubsection"],
Cell[69991, 1904, 939, 17, 31, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[70967, 1926, 253, 5, 27, "Subsubsection"],
Cell[71223, 1933, 1217, 30, 79, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[72477, 1968, 308, 6, 27, "Subsubsection"],
Cell[72788, 1976, 204, 3, 29, "Text"],
Cell[72995, 1981, 439, 11, 31, "Input"],
Cell[73437, 1994, 1785, 50, 31, "Input"],
Cell[75225, 2046, 1011, 24, 31, "Input"],
Cell[CellGroupData[{
Cell[76261, 2074, 1073, 31, 31, "Input"],
Cell[77337, 2107, 1245, 29, 46, "Output"]
}, Open  ]],
Cell[78597, 2139, 818, 22, 52, "Input"],
Cell[79418, 2163, 447, 12, 92, "Input"],
Cell[79868, 2177, 702, 16, 92, "Input"],
Cell[80573, 2195, 702, 16, 92, "Input"],
Cell[81278, 2213, 777, 17, 92, "Input"],
Cell[82058, 2232, 799, 17, 92, "Input"],
Cell[82860, 2251, 1031, 21, 92, "Input"],
Cell[83894, 2274, 1013, 20, 92, "Input"],
Cell[84910, 2296, 1077, 22, 92, "Input"]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[86048, 2325, 102, 1, 71, "Section"],
Cell[86153, 2328, 475, 11, 52, "Input"],
Cell[86631, 2341, 299, 7, 31, "Input"],
Cell[86933, 2350, 722, 16, 31, "Input"],
Cell[87658, 2368, 677, 15, 31, "Input"],
Cell[88338, 2385, 637, 17, 72, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[89012, 2407, 101, 1, 71, "Section"],
Cell[89116, 2410, 185, 2, 29, "Text"],
Cell[CellGroupData[{
Cell[89326, 2416, 69, 1, 31, "Input"],
Cell[89398, 2419, 1193, 33, 97, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[90628, 2457, 1167, 31, 72, "Input"],
Cell[91798, 2490, 847, 22, 48, "Output"],
Cell[92648, 2514, 1258, 35, 48, "Output"]
}, Open  ]],
Cell[93921, 2552, 216, 3, 29, "Text"],
Cell[94140, 2557, 881, 19, 49, "Input"],
Cell[CellGroupData[{
Cell[95046, 2580, 261, 5, 31, "Input"],
Cell[95310, 2587, 1034, 28, 61, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[96381, 2620, 263, 8, 27, "Subsubsection"],
Cell[96647, 2630, 986, 30, 49, "Input"],
Cell[97636, 2662, 1044, 29, 72, "Input"],
Cell[98683, 2693, 994, 25, 52, "Input"],
Cell[99680, 2720, 1371, 34, 55, "Input"],
Cell[101054, 2756, 1048, 30, 55, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[102139, 2791, 189, 2, 27, "Subsubsection"],
Cell[CellGroupData[{
Cell[102353, 2797, 2831, 66, 77, "Input"],
Cell[105187, 2865, 1353, 36, 48, "Output"],
Cell[106543, 2903, 3169, 85, 95, "Output"]
}, Open  ]],
Cell[109727, 2991, 282, 8, 52, "Input"],
Cell[CellGroupData[{
Cell[110034, 3003, 668, 14, 72, "Input"],
Cell[110705, 3019, 399, 6, 30, "Output"],
Cell[111107, 3027, 406, 6, 30, "Output"]
}, Open  ]],
Cell[111528, 3036, 1221, 33, 55, "Input"],
Cell[112752, 3071, 1505, 39, 55, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[114294, 3115, 122, 1, 27, "Subsubsection"],
Cell[114419, 3118, 793, 17, 92, "Input"],
Cell[115215, 3137, 813, 18, 92, "Input"],
Cell[116031, 3157, 871, 19, 92, "Input"],
Cell[116905, 3178, 802, 18, 92, "Input"],
Cell[117710, 3198, 548, 15, 92, "Input"],
Cell[118261, 3215, 746, 18, 92, "Input"],
Cell[119010, 3235, 553, 15, 95, "Input"],
Cell[119566, 3252, 695, 17, 95, "Input"],
Cell[120264, 3271, 604, 16, 95, "Input"],
Cell[120871, 3289, 606, 16, 95, "Input"],
Cell[121480, 3307, 551, 15, 92, "Input"],
Cell[122034, 3324, 646, 16, 92, "Input"],
Cell[122683, 3342, 552, 15, 95, "Input"],
Cell[123238, 3359, 647, 16, 95, "Input"],
Cell[123888, 3377, 722, 17, 95, "Input"],
Cell[124613, 3396, 555, 15, 95, "Input"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[125217, 3417, 88, 1, 71, "Section"],
Cell[125308, 3420, 130, 1, 29, "Text"],
Cell[CellGroupData[{
Cell[125463, 3425, 1304, 23, 72, "Input"],
Cell[126770, 3450, 371, 6, 30, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[127178, 3461, 261, 8, 52, "Input"],
Cell[127442, 3471, 349, 5, 23, "Print"]
}, Open  ]]
}, Open  ]]
}, Open  ]]
}
]
*)

(* End of internal cache information *)
